/*
 * OpenVINS: An Open Platform for Visual-Inertial Research
 * Copyright (C) 2019 Patrick Geneva
 * Copyright (C) 2019 Kevin Eckenhoff
 * Copyright (C) 2019 Guoquan Huang
 * Copyright (C) 2019 OpenVINS Contributors
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */


#include <string>
#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string/predicate.hpp>

#include "utils/Colors.h"



/**
 * Given a CSV file this will convert it to our text file format.
 */
void process_csv(std::string infile) {

    // Check if file paths are good
    std::ifstream file1;
    std::string line;
    file1.open(infile);
    printf("Opening file %s\n",boost::filesystem::path(infile).filename().c_str());

    // Check that it was successful
    if (!file1) {
        printf(RED "ERROR: Unable to open input file...\n" RESET);
        printf(RED "ERROR: %s\n" RESET, infile.c_str());
        std::exit(EXIT_FAILURE);
    }

    // Loop through each line of this file
    std::vector<Eigen::VectorXd> traj_data;
    std::string current_line;
    while(std::getline(file1, current_line)) {

        // Skip if we start with a comment
        if(!current_line.find("#"))
            continue;

        // Loop variables
        int i = 0;
        std::istringstream s(current_line);
        std::string field;
        Eigen::Matrix<double,8,1> data;

        // Loop through this line (timestamp(ns) tx ty tz qw qx qy qz)
        while(std::getline(s,field,',')) {
            // Skip if empty
            if(field.empty() || i >= data.rows())
                continue;
            // save the data to our vector
            data(i) = std::atof(field.c_str());
            i++;
        }

        // Only a valid line if we have all the parameters
        if(i > 7) {
            traj_data.push_back(data);
            //std::cout << std::setprecision(5) << data.transpose() << std::endl;
        }

    }

    // Finally close the file
    file1.close();

    // Error if we don't have any data
    if (traj_data.empty()) {
        printf(RED "ERROR: Could not parse any data from the file!!\n" RESET);
        printf(RED "ERROR: %s\n" RESET,infile.c_str());
        std::exit(EXIT_FAILURE);
    }
    printf("\t- Loaded %d poses from file\n",(int)traj_data.size());

    // If file exists already then crash
    std::string outfile = infile.substr(0,infile.find_last_of('.'))+".txt";
    if(boost::filesystem::exists(outfile)) {
        printf(RED "\t- ERROR: Output file already exists, please delete and re-run this script!!\n" RESET);
        printf(RED "\t- ERROR: %s\n" RESET,outfile.c_str());
        return;
    }

    // Open this file we want to write to
    std::ofstream file2;
    file2.open(outfile.c_str());
    if(file2.fail()) {
        printf(RED "ERROR: Unable to open output file!!\n" RESET);
        printf(RED "ERROR: %s\n" RESET,outfile.c_str());
        std::exit(EXIT_FAILURE);
    }
    file2 << "# timestamp(s) tx ty tz qx qy qz qw" << std::endl;

    // Write to disk in the correct order!
    for(size_t i=0; i<traj_data.size(); i++) {
        file2.precision(5);
        file2.setf(std::ios::fixed, std::ios::floatfield);
        file2 << 1e-9*traj_data.at(i)(0) << " ";
        file2.precision(6);
        file2 << traj_data.at(i)(1) << " " << traj_data.at(i)(2) << " " << traj_data.at(i)(3) << " "
              << traj_data.at(i)(5) << " " << traj_data.at(i)(6) << " " << traj_data.at(i)(7) << " " << traj_data.at(i)(4) << std::endl;
    }
    printf("\t- Saved to file %s\n",boost::filesystem::path(outfile).filename().c_str());

    // Finally close the file
    file2.close();

}





int main(int argc, char **argv) {

    // Ensure we have a path
    if(argc < 2) {
        printf(RED "ERROR: Please specify a file to convert\n" RESET);
        printf(RED "ERROR: ./format_convert <file.csv or folder\n" RESET);
        printf(RED "ERROR: rosrun ov_eval format_convert <file.csv or folder>\n" RESET);
        std::exit(EXIT_FAILURE);
    }

    // If we do not have a wildcard, then process this one csv
    if(boost::algorithm::ends_with(argv[1],"csv")) {

        // Process this single file
        process_csv(argv[1]);

    } else {

        // Loop through this directory
        boost::filesystem::path infolder(argv[1]);
        for(auto& p: boost::filesystem::recursive_directory_iterator(infolder)) {
            if(p.path().extension() == ".csv") {
                process_csv(p.path().string());
            }
        }

    }

    // Done!
    return EXIT_SUCCESS;

}



